#ifndef TRANSFORM_H
#define TRANSFORM_H
#include <ros/ros.h> 

#include<iostream>
#include "stdint.h"
#include "math.h"

#include <Eigen/Core>
#include <Eigen/Dense>
#include <tf/tf.h>
#include <tf2_eigen/tf2_eigen.h>
#include <eigen_conversions/eigen_msg.h>
#include <geometry_msgs/Pose.h>
#include <geometry_msgs/Pose2D.h>

using namespace std;

#define ROLL  0
#define PITCH 1
#define YAW   2

#define Q0    0
#define Q1    1
#define Q2    2
#define Q3    3

namespace common_lib
{
struct Quaternion
{
    float q0;
    float q1;
    float q2;
    float q3;
};

class Transform
{
private:
    /* data */
public:
    Transform();
    ~Transform();
    void EulerToQuaternion(float *euler,Quaternion *quat);
    void QuatNormalize(Quaternion *quat );
    void QuaternionToEuler(Quaternion *quat,float *euler);
    void EulerToCbn(float *euler,Eigen::Matrix3f &cbn);
    void EulerToCtb(float *euler, Eigen::Matrix3f &ctb);
    void EulerToCbh(float *euler, Eigen::Matrix3f &cbh);

    // Get XYZYawRollPitch From TransforMatirx
    Eigen::Matrix<float, 6, 1>  GetXyzyprFromTransformMatrix(const Eigen::Matrix4f& T);
    Eigen::Vector3f             GetEulerFromRotationMatrix(Eigen::Matrix3f rotation_matrix);

    Eigen::Matrix3d EulerAngle2Mat(double rx, double ry, double rz);
    Eigen::Matrix3d EulerAngle2Mat(Eigen::Vector3d eular); // eular: rad
    Eigen::Matrix4d Compose(Eigen::Vector3d positon, Eigen::Vector3d rotEular);
    Eigen::Matrix4d ComposeEuler(const double x, const double y, const double z, const double rx, const double ry, const double rz);
    Eigen::Vector3d Mat2EulerAngle(Eigen::Matrix3d mat);
    Eigen::VectorXd H2EulerAngle(Eigen::Matrix4d t);
    Eigen::Vector3d PosTF(Eigen::Vector3d & input, geometry_msgs::Pose2D & pose);
    Eigen::Vector3d PosTF(Eigen::Vector3d & input, geometry_msgs::Pose & pose);
};
}
#endif
